
#ifndef PLANNER_PKG__DIJKSTRA_HPP_
#define PLANNER_PKG__DIJKSTRA_HPP_

#include <math.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include "vector"
#include "algorithm"
#include "rclcpp/rclcpp.hpp"

namespace planner_pkg
{

#define COST_UNKNOWN_ROS 255 // 255 is unknown cost 未知成本，通常不使用，有时用于特殊标记
#define COST_OBS 254         // 254 for forbidden regions 禁止区域，致命障碍物（如：墙壁）
#define COST_OBS_ROS 253     // ROS values of 253 are obstacles 已知区域的成本值，数值越大越难以通过

#define COST_NEUTRAL 50 // Set this to "open space" value 开放空间值,中立成本值
#define COST_FACTOR 0.8 // Used for translating costs in NavFn::setCostmap() 成本转换因子

// potential defs
#define POT_HIGH 1.0e10 // 未分配单元格的势能

// priority buffers
#define PRIORITYBUFSIZE 10000 // 缓冲区大小

    struct PointNode
    {
        int x, y;
        std::shared_ptr<PointNode> parent;
        int g;
        // g 是当前节点到起始节点的距离
        PointNode(int x_val, int y_val, std::shared_ptr<PointNode> par, int g_val)
            : x(x_val), y(y_val), parent(par), g(g_val) {}
    };

    struct CompareNode
    {
        bool operator()(const std::shared_ptr<PointNode> &a, const std::shared_ptr<PointNode> &b)
        {
            return a->g > b->g; // 按 g 的最小值排序
        }
    };

    class Dijkstra
    {
    public:
        /**
         * @brief  Constructs the planner
         * @param nx The x size of the map
         * @param ny The y size of the map
         */
        Dijkstra(int nx, int ny);
        // 析构函数
        ~Dijkstra();

        /**
         * @brief  设置地图尺寸
         * @param nx The x size of the map
         * @param ny The y size of the map
         */
        void setMapSize(int nx, int ny);
        int nx, ny, ns; // size of grid, in pixels 地图尺寸

        /**
         * @brief 设置成本地图
         */
        void setCostmap(unsigned char *cmap);

        std::vector<std::shared_ptr<PointNode>> searchPath();
        

        void setStart(int *start);
        void setGoal(int *goal);

    protected:
        std::vector<std::shared_ptr<PointNode>> getPath(std::shared_ptr<PointNode> current);

        int costObs(int x, int y);

    private:
        unsigned char *costmap_; // 成本地图
        int xy_goal_tolerance_ = 1;

        int goal[2];
        int start[2];
    };

} // namespace planner_pkg

#endif // PLANNER_PKG__DIJKSTRA_HPP_
